package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.PickerState;

public class Picker extends StateObject {

	private static final int MAX_PICK_RETRY = 3;
	private int m_pickRetryCount;
	public MoveToTreasure m_moveToTreasure;
	public PickTreasure m_pickTreasure;
	public MoveBackToSniff m_moveBackToSniff;
	
	public Picker(Robot robot) {
		super(robot);
		m_moveToTreasure = new MoveToTreasure(robot);
		m_pickTreasure = new PickTreasure(robot);
		m_moveBackToSniff = new MoveBackToSniff(robot);
	}

	@Override
	public void Initialize() {
		//m_moveToTreasure.Initialize();
		//m_pickTreasure.Initialize();
		//m_moveBackToSniff.Initialize();
		m_pickRetryCount = 0;
		machineState = PickerState.START;
	}

	@Override
	public boolean IsDone() {
		return machineState == PickerState.DONE;
	}
	
	public IStateMachine start() {
		m_pickRetryCount = 0;
		machineState = PickerState.MOVE_TO_TREASURE;
		return machineState;
		
	}
	
	public IStateMachine MoveToTreasure() {
		ReturnResult returnVal = m_moveToTreasure.PerformAction();
		if (returnVal == ReturnResult.Completed) {
			if (m_moveToTreasure.isDestinationReached())
				machineState = PickerState.PICK;
			else
				machineState = PickerState.MOVE_BACK_TO_SNIFF;
		}
		return machineState;
		
	}

	public IStateMachine Pick() {
		ReturnResult returnVal =  m_pickTreasure.PerformAction();
		if (returnVal == ReturnResult.Completed/*Pick Done*/) {
			if (m_pickTreasure.IsTreasurePicked())
				robot.increaseTreasurePickCount();
			
			if (m_pickTreasure.IsTreasureDroped()) {
				++m_pickRetryCount;
				
				if (m_pickRetryCount > MAX_PICK_RETRY)
					machineState = PickerState.MOVE_BACK_TO_SNIFF;
				else
					machineState = PickerState.MOVE_TO_TREASURE;
			}
			else {
				machineState = PickerState.MOVE_BACK_TO_SNIFF;
			}
		}
		
		return machineState;
	}

	public IStateMachine MoveBackToSniff() {
		ReturnResult returnVal =  m_moveBackToSniff.PerformAction(m_moveToTreasure.getDistanceTraveled(), m_moveToTreasure.getTotalAngleTurned());
		
		if (returnVal == ReturnResult.Completed) {
			machineState = PickerState.DONE;
		}
		
		return machineState;
	}
}
